﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using static MAVLink;

internal class MavlinkParseExt : MavlinkParse
{
    static byte[] _JiYiEncoderVector = new byte[]
        {
            0xFE, 0xF6, 0xEE, 0xE6, 0xDE, 0xD6, 0xCE, 0xC6, 0xBE, 0xB6, 0xAE, 0xA6, 0x9E, 0x96, 0x8E, 0x86,
            0x7E, 0x76, 0x6E, 0x66, 0x5E, 0x56, 0x4E, 0x46, 0x3E, 0x36, 0x2E, 0x26, 0x1E, 0x16, 0x0E, 0x06,
            0xFF, 0xF7, 0xEF, 0xE7, 0xDF, 0xD7, 0xCF, 0xC7, 0xBF, 0xB7, 0xAF, 0xA7, 0x9F, 0x97, 0x8F, 0x87,
            0x7F, 0x77, 0x6F, 0x67, 0x5F, 0x57, 0x4F, 0x47, 0x3F, 0x37, 0x2F, 0x27, 0x1F, 0x17, 0x0F, 0x07,
            0xFC, 0xF4, 0xEC, 0xE4, 0xDC, 0xD4, 0xCC, 0xC4, 0xBC, 0xB4, 0xAC, 0xA4, 0x9C, 0x94, 0x8C, 0x84,
            0x7C, 0x74, 0x6C, 0x64, 0x5C, 0x54, 0x4C, 0x44, 0x3C, 0x34, 0x2C, 0x24, 0x1C, 0x14, 0x0C, 0x04,
            0xFD, 0xF5, 0xED, 0xE5, 0xDD, 0xD5, 0xCD, 0xC5, 0xBD, 0xB5, 0xAD, 0xA5, 0x9D, 0x95, 0x8D, 0x85,
            0x7D, 0x75, 0x6D, 0x65, 0x5D, 0x55, 0x4D, 0x45, 0x3D, 0x35, 0x2D, 0x25, 0x1D, 0x15, 0x0D, 0x05,
            0xFA, 0xF2, 0xEA, 0xE2, 0xDA, 0xD2, 0xCA, 0xC2, 0xBA, 0xB2, 0xAA, 0xA2, 0x9A, 0x92, 0x8A, 0x82,
            0x7A, 0x72, 0x6A, 0x62, 0x5A, 0x52, 0x4A, 0x42, 0x3A, 0x32, 0x2A, 0x22, 0x1A, 0x12, 0x0A, 0x02,
            0xFB, 0xF3, 0xEB, 0xE3, 0xDB, 0xD3, 0xCB, 0xC3, 0xBB, 0xB3, 0xAB, 0xA3, 0x9B, 0x93, 0x8B, 0x83,
            0x7B, 0x73, 0x6B, 0x63, 0x5B, 0x53, 0x4B, 0x43, 0x3B, 0x33, 0x2B, 0x23, 0x1B, 0x13, 0x0B, 0x03,
            0xF8, 0xF0, 0xE8, 0xE0, 0xD8, 0xD0, 0xC8, 0xC0, 0xB8, 0xB0, 0xA8, 0xA0, 0x98, 0x90, 0x88, 0x80,
            0x78, 0x70, 0x68, 0x60, 0x58, 0x50, 0x48, 0x40, 0x38, 0x30, 0x28, 0x20, 0x18, 0x10, 0x08, 0x00,
            0xF9, 0xF1, 0xE9, 0xE1, 0xD9, 0xD1, 0xC9, 0xC1, 0xB9, 0xB1, 0xA9, 0xA1, 0x99, 0x91, 0x89, 0x81,
            0x79, 0x71, 0x69, 0x61, 0x59, 0x51, 0x49, 0x41, 0x39, 0x31, 0x29, 0x21, 0x19, 0x11, 0x09, 0x01
        };

    static byte[] _JiYiDecoderVector = new byte[]
    {
            0xDF,0xFF,0x9F,0xBF,0x5F,0x7F,0x1F,0x3F,0xDE,0xFE,0x9E,0xBE,0x5E,0x7E,0x1E,0x3E,
            0xDD,0xFD,0x9D,0xBD,0x5D,0x7D,0x1D,0x3D,0xDC,0xFC,0x9C,0xBC,0x5C,0x7C,0x1C,0x3C,
            0xDB,0xFB,0x9B,0xBB,0x5B,0x7B,0x1B,0x3B,0xDA,0xFA,0x9A,0xBA,0x5A,0x7A,0x1A,0x3A,
            0xD9,0xF9,0x99,0xB9,0x59,0x79,0x19,0x39,0xD8,0xF8,0x98,0xB8,0x58,0x78,0x18,0x38,
            0xD7,0xF7,0x97,0xB7,0x57,0x77,0x17,0x37,0xD6,0xF6,0x96,0xB6,0x56,0x76,0x16,0x36,
            0xD5,0xF5,0x95,0xB5,0x55,0x75,0x15,0x35,0xD4,0xF4,0x94,0xB4,0x54,0x74,0x14,0x34,
            0xD3,0xF3,0x93,0xB3,0x53,0x73,0x13,0x33,0xD2,0xF2,0x92,0xB2,0x52,0x72,0x12,0x32,
            0xD1,0xF1,0x91,0xB1,0x51,0x71,0x11,0x31,0xD0,0xF0,0x90,0xB0,0x50,0x70,0x10,0x30,
            0xCF,0xEF,0x8F,0xAF,0x4F,0x6F,0x0F,0x2F,0xCE,0xEE,0x8E,0xAE,0x4E,0x6E,0x0E,0x2E,
            0xCD,0xED,0x8D,0xAD,0x4D,0x6D,0x0D,0x2D,0xCC,0xEC,0x8C,0xAC,0x4C,0x6C,0x0C,0x2C,
            0xCB,0xEB,0x8B,0xAB,0x4B,0x6B,0x0B,0x2B,0xCA,0xEA,0x8A,0xAA,0x4A,0x6A,0x0A,0x2A,
            0xC9,0xE9,0x89,0xA9,0x49,0x69,0x09,0x29,0xC8,0xE8,0x88,0xA8,0x48,0x68,0x08,0x28,
            0xC7,0xE7,0x87,0xA7,0x47,0x67,0x07,0x27,0xC6,0xE6,0x86,0xA6,0x46,0x66,0x06,0x26,
            0xC5,0xE5,0x85,0xA5,0x45,0x65,0x05,0x25,0xC4,0xE4,0x84,0xA4,0x44,0x64,0x04,0x24,
            0xC3,0xE3,0x83,0xA3,0x43,0x63,0x03,0x23,0xC2,0xE2,0x82,0xA2,0x42,0x62,0x02,0x22,
            0xC1,0xE1,0x81,0xA1,0x41,0x61,0x01,0x21,0xC0,0xE0,0x80,0xA0,0x40,0x60,0x00,0x20
    };

    private bool _IsJiYi = false;
    public bool IsJIYI => _IsJiYi;


    private void JiYiDecode(byte[] data, int offset, int len)
    {
        int pos = offset;
        for (int i = 0; i < len; i++)
        {
            data[pos + i] = _JiYiDecoderVector[data[pos+i]];
        }
    }

    private void JiYiEncode(byte[] data, int offset, int len)
    {
        int pos = offset;
        for (int i = 0; i < len; i++)
        {
            data[pos + i] = _JiYiEncoderVector[data[pos + i]];
        }
    }

    public new MAVLinkMessage ReadPacket(Stream BaseStream)
    {
        byte[] buffer = new byte[MAVLink.MAVLINK_MAX_PACKET_LEN];

        DateTime packettime = DateTime.MinValue;

        if (hasTimestamp)
        {
            byte[] datearray = new byte[8];

            int tem = BaseStream.Read(datearray, 0, datearray.Length);

            Array.Reverse(datearray);

            DateTime date1 = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);

            UInt64 dateint = BitConverter.ToUInt64(datearray, 0);

            if ((dateint / 1000 / 1000 / 60 / 60) < 9999999)
            {
                date1 = date1.AddMilliseconds(dateint / 1000);

                packettime = date1.ToLocalTime();
            }
        }

        int readcount = 0;

        while (readcount <= MAVLink.MAVLINK_MAX_PACKET_LEN)
        {
            // read STX byte
            ReadWithTimeout(BaseStream, buffer, 0, 1);

            if (buffer[0] == MAVLink.MAVLINK_STX || buffer[0] == MAVLINK_STX_MAVLINK1 || buffer[0] == MAVLINK_STX_JIYI_MAVLINK1)
                break;

            if (buffer[0] == _JiYiEncoderVector[MAVLINK_STX_JIYI_MAVLINK1])
            {
                buffer[0] = _JiYiDecoderVector[buffer[0]];
                _IsJiYi = true;
                break;
            }

            readcount++;
        }

        if (readcount >= MAVLink.MAVLINK_MAX_PACKET_LEN)
        {
            return null;
            throw new InvalidDataException("No header found in data");
        }

        byte headerlength = buffer[0] == MAVLINK_STX ? MAVLINK_CORE_HEADER_LEN : MAVLINK_CORE_HEADER_MAVLINK1_LEN;
        var headerlengthstx = headerlength + 1;

        // read header
        try
        {
            ReadWithTimeout(BaseStream, buffer, 1, headerlength);
        }
        catch (EndOfStreamException)
        {
            return null;
        }

        if(_IsJiYi)
        {// 如果是极翼，则进行解码
            JiYiDecode(buffer, 1, headerlength);
        }
        

        // packet length
        int lengthtoread = 0;
        if (buffer[0] == MAVLINK_STX)
        {
            lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - magic - length
            if ((buffer[2] & MAVLINK_IFLAG_SIGNED) > 0)
            {
                lengthtoread += MAVLINK_SIGNATURE_BLOCK_LEN;
            }
        }
        else
        {
            lengthtoread = buffer[1] + headerlengthstx + 2 - 2; // data + header + checksum - U - length    
        }

        try
        {
            //read rest of packet
            ReadWithTimeout(BaseStream, buffer, headerlengthstx, lengthtoread - (headerlengthstx - 2));
            if (_IsJiYi)
            {
                JiYiDecode(buffer, headerlengthstx, lengthtoread - (headerlengthstx - 2));
            }
        }
        catch (EndOfStreamException)
        {
            return null;
        }

        

        // resize the packet to the correct length
        Array.Resize<byte>(ref buffer, lengthtoread + 2);

        MAVLinkMessage message = new MAVLinkMessage(buffer, packettime);

        // calc crc
        ushort crc = MavlinkCRC.crc_calculate(buffer, buffer.Length - 2);

        // calc extra bit of crc for mavlink 1.0+
        if (message.header == MAVLINK_STX || message.header == MAVLINK_STX_MAVLINK1 || message.header == MAVLINK_STX_JIYI_MAVLINK1)
        {
            crc = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo(message.msgid).crc, crc);
        }

        // check crc
        if ((message.crc16 >> 8) != (crc >> 8) ||
                  (message.crc16 & 0xff) != (crc & 0xff))
        {
            badCRC++;
            // crc fail
            //if (IsJIYI == false)
            //    return null;
            //else
                Console.WriteLine($"*** Bad CRC message: {message}");
            return null;
        }

        return message;
    }

    public new byte[] GenerateMAVLinkPacket10(MAVLINK_MSG_ID messageType, object indata, byte sysid = 255, byte compid = (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER, int sequence = -1)
    {
        byte[] data;

        data = MavlinkUtil.StructureToByteArray(indata);

        byte[] packet = new byte[data.Length + 6 + 2];

        packet[0] = _IsJiYi ? MAVLINK_STX_JIYI_MAVLINK1 : MAVLINK_STX_MAVLINK1;
        packet[1] = (byte)data.Length;
        packet[2] = (byte)packetcount;
        if (sequence != -1)
            packet[2] = (byte)sequence;

        packetcount++;

        packet[3] = sysid; // this is always 255 - MYGCS
        packet[4] = compid;
        packet[5] = (byte)messageType;


        int i = 6;
        foreach (byte b in data)
        {
            packet[i] = b;
            i++;
        }

        ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);

        checksum = MavlinkCRC.crc_accumulate(MAVLINK_MESSAGE_INFOS.GetMessageInfo((uint)messageType).crc, checksum);

        byte ck_a = (byte)(checksum & 0xFF); ///< High byte
        byte ck_b = (byte)(checksum >> 8); ///< Low byte

        packet[i] = ck_a;
        i += 1;
        packet[i] = ck_b;
        i += 1;

        if (_IsJiYi)
            JiYiEncode(packet, 0, packet.Length);

        return packet;
    }

}
